package com.asa.control_theory;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

import com.asa.hutils.Hanglieshi;
import com.asa.hutils.MathHUtils;
import com.asa.hutils.Ni;
import com.asa.numanaly.F;
import com.asa.numanaly.I;

public class D {
	
	/**
	 * 可控性 		Controllability		（LTI）线性时不变
	 * 
	 * 如图 control_theory-D01.png 的小车
	 * 
	 * 输入：u = F(t)
	 * 输出：小车1移动的距离x1，小车2移动的距离x2
	 * 小车1的质量：m1
	 * 小车2的质量：m2
	 * 两个小车之间的弹簧系数 k
	 * 
	 * 那么我们可以通过控制u来控制x1，x2，x1`，x2`吗？
	 * 
	 * 标准型
	 * X` = A*X +B*U
	 * 
	 * 具体会如何变化我们采用离散的方式来做：
	 * 		X(k+1) = A * Xk + B * Uk
	 * 	step:
	 * 	1	k=0		X1 = A*X0 +B*U0 = B*U0
	 * 	2	k=1		X2 = A*X1 +B*U1 = A*B*U0 + B*U1
	 * 	3	k=2		X3 = A*X2 +B*U2 = A*A*B*U0 + A*B*U1 + B*U2
	 * 	.	.
	 * 	.	.
	 * 	.	.
	 * 	n	k=n-1	Xn = A*X(n-1) +B*U(n-1) = A^(n-1)*B*U0 + ... + A*B*U(n-2) + B*U(n-1)
	 * 
	 * 	也就是说，把Xn换算成矩阵式：
	 * 		Xn = {B,A*B,...,A^(n-1)*B}
	 * 			*
	 * 			{
	 * 				U(n-1),
	 * 				U(n-2),
	 * 				.
	 * 				.
	 * 				.
	 * 				U0
	 * 			}
	 * 
	 * 		经过n步之后，X0=0 ===》 Xn
	 * 		{B,A*B,...,A^(n-1)*B}我们把它叫做C0,C0∈R^(n×n) 
	 * 		{
	 * 				U(n-1),
	 * 				U(n-2),
	 * 				.
	 * 				.
	 * 				.
	 * 				U0
	 * 		}我们把它叫做U					其实在这个地方，我有个犯罪的想法，如果A是一个循环矩阵呢？	那么这个就变成了不可控的了，定义上是这样说的。
	 * 		
	 * 		A∈R^(n×n) ,B∈R^(n×r)
	 * 
	 * 		若U有解		
	 * 			Rank(C0) = n	即为，一个系统如果可控的话，那么C0矩阵的秩一定是等于n的
	 * 
	 * 
	 * 		-----------------------------------	
	 * 		例如这样的状态矩阵的系统
	 * 		X` = 
	 * 			{
	 * 				{2,0},
	 * 				{1,1}
	 * 			}*
	 * 			X
	 * 			+
	 * 			{
	 * 				1,
	 * 				1
	 * 			}*
	 * 			U
	 * 
	 * 		C0 = {B,A*B} = 
	 * 			{
	 * 				{1 , 2*1+0},
	 * 				{1 , 1+1}
	 * 			}
	 * 			=
	 * 			{
	 * 				{1 , 2},
	 * 				{1 , 1}
	 * 			}
	 * 		
	 * 		这里的C0的秩，Rank(C0) = 1	< n	这样的系统就是不可控的
	 * 
	 * 		
	 * 		系统2：
	 * 		X` = 
	 * 			{
	 * 				{0,1},
	 * 				{0,0}
	 * 			}*
	 * 			X
	 * 			+
	 * 			{
	 * 				0,
	 * 				1
	 * 			}*
	 * 			U
	 * 		
	 * 		C0 = {B,A*B} = 
	 * 			{
	 * 				{0 , 1},
	 * 				{1 , 0}
	 * 			}
	 * 		Rank(C0) = 2	= n	这样的系统就是可控的
	 * 
	 * 		所谓的可控，是从一个X0到达另一个X1，但这个中间路径，我们是无法处理的
	 * 		
	 * 		---------------------------------------
	 * 
	 * 		
	 * 		对于上面的 两个小车模型
	 * 	
	 * 			m2 * x2`` = k * (x1 - x2)
	 * 			m1 * x1`` = U - k * (x1 - x2)
	 * 
	 * 		==>
	 * 			x2`` = 100*x1 - 100*x2
	 * 			x1`` = U - 100*x1 + 100*x2
	 * 
	 * 		令：Z1 = x1,Z2 = Z1`,Z3 = X2,Z4 = Z3`		
	 * 		Z1 ,Z2 ,Z3 ,Z4 它们可以组成一个矩阵
	 * 		我们可以来计算矩阵的秩，如果这个秩是小于4的，那么就是不可控的
	 * 		这里的这个算出来是可控的。
	 * 		
	 * 		A = {
	 * 				{0,1,0,0},
	 * 				{-100,0,100,0},
	 * 				{0,0,0,1},
	 * 				{100,0,-100,0}
	 * 			}
	 * 		B = {
	 * 				0,
	 * 				1,
	 * 				0,
	 * 				0-
	 * 			}
	 * 
	 * 
	 * 
	 * 
	 * 
	 * 
	 * 
	 * 
	 * 	CTRB  
	 * 	Compute the controllability matrix
	 * 
	 * matlab中的对能控判断的代码，下面会实现自己的方法
	 * 	C0 = ctrb(A,B)
	 * 	Rank(C0)
	 * 
	 * 
	 * 
	 */
	
	
	
	
	
	
	/**
	 * 计算能控矩阵
	 * 
	 * 在MATLAB中ctrb方法来计算能控矩阵
	 * 
	 * 这个n就是状态矩阵的n×n，中的那个n
	 * 
	 * {B,A*B,...,A^(n-1)*B}我们把它叫做C0,C0∈R^(n×n) 
	 * 
	 * 
	 * @param A	状态矩阵
	 * @param b	传入参数		我是按照b是n×m矩阵设计的，不过还没有测n×m，只有n×1是一个例子里给的，应该支持n×m
	 * @return	能控矩阵
	 * 
	 */

	public static double[][] ctrb(double[][] A,double[][] b) {
		
		int n = A.length;
		double[][] result = new double[n][n*b[0].length];
		
		for (int i = 0; i < b.length; i++) {
			for (int j = 0; j < b[i].length; j++) {
				result[i][0] = b[i][j];
			}
		}
		
		double[][] process = A;
		
		for (int k = 1; k < n; k++) {//这个完成的时候，我感觉好好，感觉“真的有这么丝滑吗？”，我实在是太帅了
			double[][] chenfa = MathHUtils.chenfa(process, b);
			
			for (int i = 0; i < chenfa.length; i++) {
				for (int j = 0; j < chenfa[i].length; j++) {
					result[i][k] = chenfa[i][j];
				}
			}
			process = MathHUtils.chenfa(process, A);	
		}
		
		return result;
		
		
		
		
//		double[][] chenfa = MathHUtils.chenfa(A, b);
//		
//		for (int i = 0; i < chenfa.length; i++) {
//			for (int j = 0; j < chenfa[i].length; j++) {
//				System.out.print(chenfa[i][j] + "  ");
//			}
//			System.out.println();
//		}
//		
//		System.out.println("=====================");
//		
//		
//		double[][] chenfa2 = MathHUtils.chenfa(A, A);
//		double[][] chenfa3 = MathHUtils.chenfa(chenfa2, b);
//		
//		for (int i = 0; i < chenfa3.length; i++) {
//			for (int j = 0; j < chenfa3[i].length; j++) {
//				System.out.print(chenfa3[i][j] + "  ");
//			}
//			System.out.println();
//		}
//		System.out.println("=====================");
//
//		double[][] chenfa4 = MathHUtils.chenfa(chenfa2, A);
//		double[][] chenfa5 = MathHUtils.chenfa(chenfa4, b);
//
//		for (int i = 0; i < chenfa5.length; i++) {
//			for (int j = 0; j < chenfa5[i].length; j++) {
//				System.out.print(chenfa5[i][j] + "  ");
//			}
//			System.out.println();
//		}
//		System.out.println("=====================");
//
//		for (int i = 0; i < result.length; i++) {
//			for (int j = 0; j < result[i].length; j++) {
//				System.out.print(result[i][j] + "  ");
//			}
//			System.out.println();
//		}
	}
	
	/**
	 * 由于技术能力有限，我也没法去求出矩阵的秩是多少，但是我可以知道是否是满秩的（其实还是有办法的，但感觉这个方法太恶心了，不优美，就不写了）
	 * 这个是通过计算行列式的值来判断是否满秩
	 * @param A
	 * @return
	 */
	public static boolean isrankall(double[][] A){
		
		 double value_one = Ni.hangleishivalue(A);
		
		 if (value_one==0) {
			return false;
		}
		 
		return true;
	}
	
	
	
	
	
	/**
	 * 求解矩阵的秩
	 * @throws Exception 
	 */
	public static void rank(double[][] A) throws Exception {
		
		
		
//		double[] fangchengzu1 = 
//				F.fangchengzu6(A);
		
//		for (int i = 0; i < fangchengzu1.length; i++) {
//			System.out.println(fangchengzu1[i]);
//		}
		
				
		//貌似是没有现存的方法，我这里自己想一个
		//首先对矩阵中的列进排序，排序规则遵循的是，每一个列项中，从0 index开始，对比，0的就排前面
		
//		A = MathHUtils.zhuanzhi(A);
		
		int n = A[0].length;
		
		double[][] asa =new double[A.length][];
		
		List<Pick> picklist = new ArrayList<Pick>();
//		List<Pick> sortpicklist = new ArrayList<Pick>();

		System.out.println("-----------");
		for (int i = 0; i < A.length; i++) {
			Pick pick = new Pick();
			pick.item = A[i];
			picklist.add(pick);
			
		}
		
//		int size = pick.size();
		
		
		Collections.sort(picklist);
		
		for (int i = 0; i < picklist.size(); i++) {
			asa[i] =  picklist.get(i).item;
		}
		
		
		
		
		for (int i = 0; i < asa.length; i++) {
			for (int j = 0; j < asa[i].length; j++) {
				System.out.print(asa[i][j] + "  ");
			}
			System.out.println("**");
		}
		//至此，这样的asa是排好序了的
		
		int index = 0;
//		while (true) {
//			
//			
//			
//			
//			
//		}
		
		
		System.out.println("============asasasasasa================");

//		double[][] tezhengzhi5_gai = F.fangchengzu4(A)[0];
		
		
		
//		F.fangchengzu2(a, b)
		double[][] fangchengzu000 = F.fangchengzu000(asa);
		
		for (int i = 0; i < fangchengzu000.length; i++) {
			for (int j = 0; j < fangchengzu000[i].length; j++) {
				System.out.print(fangchengzu000[i][j] + "  ");
			}
			System.out.println("**");
		}
		
		
		double[][] chenfa = MathHUtils.chenfa(A, fangchengzu000);
		for (int i = 0; i < chenfa.length; i++) {
			for (int j = 0; j < chenfa[i].length; j++) {
				System.out.print(chenfa[i][j] + "  ");
			}
			System.out.println("**");
		}
		
	}
	
	
	
	private static class Pick implements Comparable<Pick>{
		
		double[] item; 
		
		@Override
		public int compareTo(Pick o) {
			// TODO Auto-generated method stub
			double[] item2 = o.item;
			for (int i = 0; i < item2.length; i++) {
				double d1 = item[i];
				double d2 = item2[i];
				
				if (d1==0&&d2!=0) {
					return 1;
				}else if(d1!=0&&d2==0){
					return -1;
				}else if(d1==0&&d2==0){
					continue;
				}
			}

			return 0;
		}
		
	}
	
	
	
	
	
	
	
	
	public static void main(String[] args) throws Exception {
		
		
		double[][] A = {
				{0,1,0,0},
				{-100,0,100,0},
				{0,0,0,1},
				{100,0,-100,0}
		};
		double[] B = {
				  				0,
				  				1,
				  				0,
				 				0
				  			};
		double[][] b = {B};
		b = MathHUtils.zhuanzhi(b);
		
		double[][] result = ctrb(A,b);
		
		for (int i = 0; i < result.length; i++) {
			for (int j = 0; j < result[i].length; j++) {
				System.out.print(result[i][j] + "  ");
			}
			System.out.println("**");
		}
		System.out.println("---------------");
//		double[][] ni = Ni.getNi(A);
		
		rank(result);//这里有个现象，就是状态矩阵A是非满秩的，而控制矩阵result是满秩的
		
		
		System.out.println(isrankall(result));
		
	}
	
	
	
	
	
	
	

}
